package physics;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;


public class World {
	private ArrayList<Shape> shapes = new ArrayList<Shape>();
	List<Shape> syncShapes = Collections.synchronizedList(shapes);
	private double stepSize = 1;
	
	public World() {
		this.stepSize = 1;
	}
	
	public World(double stepSize) {
		this.stepSize = stepSize;
	}
	
	public World(ArrayList<Shape> shapes) {
		this.stepSize = 1;
		this.shapes = shapes;
	}
	
	public World(double stepSize, ArrayList<Shape> shapes) {
		this.stepSize = stepSize;
		this.shapes = shapes;
	}
	
	public void iterateWorld() {
		synchronized(shapes) {
			Shape collWith;
			for(Shape shape : shapes) {
				if((collWith = willCollide(shape)) != null) {
					collide(shape, collWith);
				}
				if(!shape.isFixed()) {
					if(willCollide(shape) == null) {
						//shape.posn = move(shape).posn;
						shape.changePosn(shape.vel.multiply(stepSize));
						shape.vel = shape.vel.add(shape.acc.multiply(stepSize));
					}
					shape.vel.y -= shape.gravity * stepSize;
				} else {
					shape.vel = new Vector(0, 0);
				}
			}
		}
	}
	
	/**
	 * This method will check whether an object (or its bounding circle if a
	 *  polygon) will collide with any other Shapes in the world.
	 * @param moving Shape to check for collisions
	 * @return Whether moving will hit anything in one movement.
	 */
	public Shape willCollide(Shape moving) {
		Shape collision = null;
		if(moving instanceof Circle) {
			Circle moved = (Circle)move(moving);
			for(Shape collWith : shapes) {	
				//If the last iteration found a collision, exit and return.
				if(collision != null) {
					break;
				}
				
				//Don't need to check if we collide with ourself.
				if(collWith.equals(moving)) {
					continue;
				}
				
				//Circle vs Circle detection
				if(collWith instanceof Circle) {
					Circle collWithMoved = (Circle)move(collWith);
					if(willCollideCircleCircle((Circle)moved, collWithMoved)) {
						collision = collWith;
					}
					
				//Circle vs. Rectangle detection
				} else if(collWith instanceof Rectangle) {
					Rectangle collWithMoved = (Rectangle)move(collWith);
					if(willCollideCircleRectangle((Circle)moved, collWithMoved)) {
						collision = collWith;
					}
				}
			}
		} else if (moving instanceof Rectangle) {
//			System.out.println("Rect is " + ((Rectangle)moving).botLeftPosn.toString() + 
//					" => " + ((Rectangle)moving).topRightPosn.toString());
			Rectangle moved = (Rectangle)move(moving);
			for(Shape collWith : shapes) {
				//If the last iteration found a collision, exit and return.
				if(collision != null) {
					System.out.println("Collision");
					break;
				}

				//Don't need to check if we collide with ourself.
				if(collWith.equals(moving)) {
					continue;
				}
				
				//Rectangle vs Rectangle detection
				if(collWith.isRectangle()) {
					Rectangle collWithMoved = (Rectangle)move(collWith);
					if(willCollideRectRect((Rectangle)moved, collWithMoved)) {
						System.out.println("RECTRECT");
						collision = collWith;
					}
					
				//Rectangle vs. Circle detection
				} else if(collWith instanceof Circle) {
					Circle collWithMoved = (Circle)move(collWith);
					if(willCollideCircleRectangle((Circle)collWithMoved, (Rectangle)moved)) {
						System.out.println("CIRCLERECT");
						collision = collWith;
					}
				}
			}
		}
		return collision;
	}

	/**
	 * This will update moving and collideWith's velocities to that which they would be after the collision.
	 * @param moving First object in collision
	 * @param collideWith Second object in collision
	 */
	private void collide(Shape moving, Shape collideWith) {
		if(moving.isCircle() && collideWith.isCircle()) {
			collideCircles(moving, collideWith);
		} else if (moving.isCircle() && collideWith.isRectangle()) {
			collideWithRect(moving, collideWith);
		} else if (moving.isRectangle() && collideWith.isCircle()) {
			collideWithRect(collideWith, moving);
		} else if (moving.isRectangle() && collideWith.isRectangle()) {
			collideRectangles(moving, collideWith);
		}
	}
	
	private void collideRectangles(Shape moving, Shape collideWith) {
		int cirInd = shapes.indexOf(moving);
		int rectInd = shapes.indexOf(collideWith);
		double stepSizeHolder = stepSize;
		stepSize = stepSize / 10;
		
		Shape backtrack = moving;
		
		for(int i = 0; i > -10; i--) {
			backtrack = move(backtrack);
			if(willCollide(backtrack) != null) {
				moving = backtrack;
				break;
			}
		}

		System.out.println("RectPosn is " + collideWith.posn);
		double dy = collideWith.posn.y - moving.posn.y;
		double dx = collideWith.posn.x - moving.posn.x;
		double absdy = Math.abs(dy);
		double absdx = Math.abs(dx);
		System.out.println("RectRect Difference is (" + dx + ", " + dy + ").");

		Vector unitNormal, unitTangent;
		if(absdy > absdx) {
			unitNormal = new Vector(0, 1);
			unitTangent = new Vector(-1, 0);
			if(moving.posn.y < collideWith.posn.y) {
				unitNormal.multiply(-1);
				unitTangent.multiply(-1);
			}
		} else if(absdy < absdx) {
			unitNormal = new Vector(1, 0);
			unitTangent = new Vector(0, 1);
			if(moving.posn.x > collideWith.posn.x) {
				unitNormal.multiply(-1);
				unitTangent.multiply(-1);
			}
		} else {
			double dist = Math.sqrt(dy * dy + dx * dx);
			double unitNormx = dx / dist;
			double unitNormy = dy / dist;
			unitNormal = new Vector(unitNormx, unitNormy);
			unitTangent = new Vector(-1 * unitNormy, unitNormx);
//			Shape moving1 = shapes.get(cirInd);
//			Shape collWith1 = shapes.get(rectInd);
//			moving1.setPosn(moving.posn);
//			moving1.setVel(moving1.vel.multiply(new Vector(-1, -.9)));
//			collWith1.setVel(collWith1.vel.multiply(new Vector(-1, -.9)));
//			return;
		}
		
		double movingNormScalar = unitNormal.dot(moving.vel);
		double movingTangScalar = unitTangent.dot(moving.vel);
		double collWithNormScalar = unitNormal.dot(collideWith.vel);
		double collWithTangScalar = unitTangent.dot(collideWith.vel);
		
		double postMovingNormScalar = (movingNormScalar * (moving.mass - 
				collideWith.mass) + 2 * (collideWith.mass * collWithNormScalar)) / 
				(moving.mass + collideWith.mass);
		
		double postCollWithNormScalar = (collWithNormScalar * (collideWith.mass - 
				moving.mass) + 2 * (moving.mass * movingNormScalar)) / 
				(moving.mass + collideWith.mass);
		
		Vector postMovingNorm = unitNormal.multiply(postMovingNormScalar);
		Vector postCollWithNorm = unitNormal.multiply(postCollWithNormScalar);
		Vector postMovingTang = unitTangent.multiply(movingTangScalar);
		Vector postCollWithTang = unitTangent.multiply(collWithTangScalar);
		
		Vector movingFinal = postMovingNorm.add(postMovingTang);
		Vector collWithFinal = postCollWithNorm.add(postCollWithTang);

		shapes.get(cirInd).setPosn(moving.posn);
		shapes.get(cirInd).setVel(movingFinal.multiply(new Vector(1, .9)));
		shapes.get(rectInd).setVel(collWithFinal.multiply(new Vector(1, .9)));
		
//		System.out.println(cirInd + " => " + shapes.get(cirInd).getVel());
//		System.out.println(rectInd + " => " + shapes.get(rectInd).getVel());
		
		stepSize = stepSizeHolder;
		
	}
	
	private void collideWithRect(Shape cir, Shape rect) {
		int cirInd = shapes.indexOf(cir);
		int rectInd = shapes.indexOf(rect);
		double stepSizeHolder = stepSize;
		stepSize = stepSize / 10;
		
		Shape backtrack = cir;
		
		for(int i = 0; i > -10; i--) {
			backtrack = move(backtrack);
			if(willCollide(backtrack) != null) {
				cir = backtrack;
				break;
			}
		}

		System.out.println("RectPosn is " + rect.posn);
		double dy = rect.posn.y - cir.posn.y;
		double dx = rect.posn.x - cir.posn.x;
		double absdy = Math.abs(dy);
		double absdx = Math.abs(dx);
		System.out.println("CirRect Difference is (" + dx + ", " + dy + ").");

		Vector unitNormal, unitTangent;
		if(absdy > absdx) {
			System.out.println("Horizontal!");
			unitNormal = new Vector(0, 1);
			unitTangent = new Vector(-1, 0);
			if(cir.posn.y < rect.posn.y) {
				unitNormal.multiply(-1);
				unitTangent.multiply(-1);
			}
		} else if(absdy < absdx) {
			System.out.println("Vertical!");
			unitNormal = new Vector(1, 0);
			unitTangent = new Vector(0, 1);
			if(cir.posn.x > rect.posn.x) {
				unitNormal.multiply(-1);
				unitTangent.multiply(-1);
			}
		} else {
			double dist = Math.sqrt(dy * dy + dx * dx);
			double unitNormx = dx / dist;
			double unitNormy = dy / dist;
			unitNormal = new Vector(unitNormx, unitNormy);
			unitTangent = new Vector(-1 * unitNormy, unitNormx);
		}
		
		double movingNormScalar = unitNormal.dot(cir.vel);
		double movingTangScalar = unitTangent.dot(cir.vel);
		double collWithNormScalar = unitNormal.dot(rect.vel);
		double collWithTangScalar = unitTangent.dot(rect.vel);
		
		double postMovingNormScalar = (movingNormScalar * (cir.mass - 
				rect.mass) + 2 * (rect.mass * collWithNormScalar)) / 
				(cir.mass + rect.mass);
		
		double postCollWithNormScalar = (collWithNormScalar * (rect.mass - 
				cir.mass) + 2 * (cir.mass * movingNormScalar)) / 
				(cir.mass + rect.mass);
		
		Vector postMovingNorm = unitNormal.multiply(postMovingNormScalar);
		Vector postCollWithNorm = unitNormal.multiply(postCollWithNormScalar);
		Vector postMovingTang = unitTangent.multiply(movingTangScalar);
		Vector postCollWithTang = unitTangent.multiply(collWithTangScalar);
		
		Vector movingFinal = postMovingNorm.add(postMovingTang);
		Vector collWithFinal = postCollWithNorm.add(postCollWithTang);

		shapes.get(cirInd).setPosn(cir.posn);
		shapes.get(cirInd).setVel(movingFinal.multiply(new Vector(1, .9)));
		shapes.get(rectInd).setVel(collWithFinal.multiply(new Vector(1, .9)));
		
//		System.out.println(cirInd + " => " + shapes.get(cirInd).getVel());
//		System.out.println(rectInd + " => " + shapes.get(rectInd).getVel());
		
		stepSize = stepSizeHolder;
		
	}
	
	private void collideCircles(Shape moving, Shape collideWith) {
		Shape movingForFinding = moving;
		Shape collideForFinding = collideWith;
		double stepSizeHolder = stepSize;
		stepSize = stepSize / 10;
		
		Shape backtrack = new Shape();
		
		for(int i = 0; i > -10; i--) {
			backtrack = move(moving);
			if(willCollide(backtrack) != null) {
				moving = backtrack;
				break;
			}
		}
		
		double dy = collideWith.posn.y - moving.posn.y;
		double dx = collideWith.posn.x - moving.posn.x;
		double dist = Math.sqrt(dy * dy + dx * dx);
		double unitNormx = dx / dist;
		double unitNormy = dy / dist;
		Vector unitNormal = new Vector(unitNormx, unitNormy);
		Vector unitTangent = new Vector(-1 * unitNormy, unitNormx);
		System.out.println("Normal: " + unitNormal.x + ", " + unitNormal.y);
		System.out.println("Tangent: " + unitTangent.x + ", " + unitTangent.y);
		double movingNormScalar = unitNormal.dot(moving.vel);
		double movingTangScalar = unitTangent.dot(moving.vel);
		double collWithNormScalar = unitNormal.dot(collideWith.vel);
		double collWithTangScalar = unitTangent.dot(collideWith.vel);
		
		double postMovingNormScalar = (movingNormScalar * (moving.mass - 
				collideWith.mass) + 2 * (collideWith.mass * collWithNormScalar)) / 
				(moving.mass + collideWith.mass);
		
		double postCollWithNormScalar = (collWithNormScalar * (collideWith.mass - 
				moving.mass) + 2 * (moving.mass * movingNormScalar)) / 
				(moving.mass + collideWith.mass);
		
		Vector postMovingNorm = unitNormal.multiply(postMovingNormScalar);
		Vector postCollWithNorm = unitNormal.multiply(postCollWithNormScalar);
		Vector postMovingTang = unitTangent.multiply(movingTangScalar);
		Vector postCollWithTang = unitTangent.multiply(collWithTangScalar);
		
		Vector movingFinal = postMovingNorm.add(postMovingTang);
		Vector collWithFinal = postCollWithNorm.add(postCollWithTang);
		
		int movingInd = shapes.indexOf(movingForFinding);
		int collInd = shapes.indexOf(collideForFinding);
		shapes.get(movingInd).setVel(movingFinal);
		shapes.get(collInd).setVel(collWithFinal);
		stepSize = stepSizeHolder;
		
	}
	
	/**
	 * Moves an object by its velocity once.
	 * @param shape Shape to move
	 */
	public Shape move(Shape shape) {
//		if(shape instanceof Circle) {
//			Vector newPosn = new Vector(shape.posn.x + (stepSize * shape.getVel().x),
//					shape.posn.y + (stepSize * (shape.getVel().y + shape.getGravity())));
//			return new Circle(newPosn, ((Circle) shape).getRadius(), shape.getVel(), 
//					shape.getAcc(), shape.getGravity(), shape.isFixed());
//		} else if(shape instanceof Rectangle) {
//			Vector newBLPosn = new Vector(((Rectangle)shape).botLeftPosn.x + (stepSize * shape.getVel().x),
//					((Rectangle)shape).botLeftPosn.y + (stepSize * (shape.getVel().y +
//					shape.getGravity())));
//			Vector newTRPosn = new Vector(((Rectangle)shape).topRightPosn.x + (stepSize * shape.getVel().x),
//					((Rectangle)shape).topRightPosn.y + (stepSize * shape.getVel().y + 
//					shape.getGravity()));
//			return new Rectangle(newBLPosn, newTRPosn, shape.getVel(), shape.getAcc(), 
//					shape.getGravity(), shape.isFixed());
//		}
//		return new Shape();
		return shape.move(shape.vel.multiply(stepSize));
	}
	
//	/**
//	 * This method will be called on Polygons if the bounding circle for moving
//	 * detects a collision.
//	 * 
//	 * @param moving Shape to check
//	 * @return Whether shape will hit anything in one movement
//	 */
//	private boolean willCollidePolygonDetailed(Shape moving) {
//		//TODO Stubbed
//		return true;
//	}
	
	/**
	 * Checks for collisions between two circles
	 * @param cir The first circle to check
	 * @param collWith The second circle to check against.
	 * @return collWith if the circles collide, null if they do not.
	 */
	private boolean willCollideCircleCircle(Circle cir, Circle collWith) {
		double dx = collWith.posn.x - cir.posn.x;
		double dy = collWith.posn.y - cir.posn.y;
		double colDist = cir.getRadius() + collWith.getRadius();
		if(dx * dx + dy * dy <= colDist * colDist) {
			return true;
		} else {
			return false;
		}
	}
	
	/**TODO
	 * Checks for collisions between a circle and a rectangle
	 * @param cir The circle to check for a collision with.
	 * @param rect The rectangle to check for a collision with.
	 * @return 
	 */
	private boolean willCollideCircleRectangle(Circle cir, Rectangle rect) {
		//Distance from the center of cir to the center of rect
		double dx = Math.abs(cir.posn.x - (rect.topRightPosn.x - rect.getWidth()/2));
		double dy = Math.abs(cir.posn.y - (rect.topRightPosn.y - rect.getHeight()/2));
		
		if(cir.posn.x >= rect.botLeftPosn.x && 
				cir.posn.x <= rect.topRightPosn.x) {
			//Center of circle is inside rectangle
			if(cir.posn.y >= rect.botLeftPosn.y && cir.posn.y <= rect.topRightPosn.y) {
				return true;
			//Center of circle is either directly above or below rectangle
			} else {
				return dy < cir.getRadius() + rect.getHeight()/2;
			}
		} else if(cir.posn.x < rect.botLeftPosn.x) {
			//Circle is directly to the left of the rectangle
			if(cir.posn.y >= rect.botLeftPosn.y && cir.posn.y <= rect.topRightPosn.y) {
				return dx < cir.getRadius() + rect.getWidth()/2;
			//Circle is above and to the left of the rectangle
			} else if (cir.posn.y > rect.topRightPosn.y){
				double toTLX = cir.posn.x - rect.botLeftPosn.x;
				double toTLY = cir.posn.y - rect.topRightPosn.y;
				return toTLX * toTLX + toTLY * toTLY < cir.getRadius() * cir.getRadius();
			//Circle is below and to the left of the rectangle
			} else {
				double toBLX = cir.posn.x - rect.botLeftPosn.x;
				double toBLY = cir.posn.y - rect.botLeftPosn.y;
				return toBLX * toBLX + toBLY * toBLY < cir.getRadius() * cir.getRadius();
			}
		} else {
			//Circle is directly to the right of the rectangle
			if(cir.posn.y >= rect.botLeftPosn.y && cir.posn.y <= rect.topRightPosn.y) {
				return dx < cir.getRadius() + rect.getWidth()/2;
			//Circle is above and to the right of the rectangle
			} else if (cir.posn.y > rect.topRightPosn.y){
				double toTRX = cir.posn.x - rect.topRightPosn.x;
				double toTRY = cir.posn.y - rect.topRightPosn.y;
				return toTRX * toTRX + toTRY * toTRY < cir.getRadius() * cir.getRadius();
			//Circle is below and to the right of the rectangle
			} else {
				double toBRX = cir.posn.x - rect.botLeftPosn.x;
				double toBRY = cir.posn.y - rect.topRightPosn.y;
				return toBRX * toBRX + toBRY * toBRY < cir.getRadius() * cir.getRadius();
			}
		}
	}
	
	/**
	 * 
	 * @param rectangle
	 * @param collWith
	 * @return
	 */
	private boolean willCollideRectRect(Rectangle rect,
			Rectangle collWith) {
		
		return (rect.topRightPosn.x >= collWith.botLeftPosn.x && //Top right corner
				rect.topRightPosn.x <= collWith.topRightPosn.x &&
				rect.topRightPosn.y >= collWith.botLeftPosn.y &&
				rect.topRightPosn.y <= collWith.topRightPosn.y) ||
				
				(rect.botLeftPosn.x >= collWith.botLeftPosn.x && //Bottom left corner
				rect.botLeftPosn.x <= collWith.topRightPosn.x &&
				rect.botLeftPosn.y >= collWith.botLeftPosn.y && 
				rect.botLeftPosn.y <= collWith.topRightPosn.y) ||
				
				(rect.botLeftPosn.x >= collWith.botLeftPosn.x && //Top left corner
				rect.botLeftPosn.x <= collWith.topRightPosn.x && 
				rect.topRightPosn.y >= collWith.botLeftPosn.y &&
				rect.topRightPosn.y <= collWith.topRightPosn.y) ||
						
				(rect.topRightPosn.x >= collWith.botLeftPosn.x && //Bottom right corner
				rect.topRightPosn.x <= collWith.topRightPosn.x && 
				rect.botLeftPosn.y >= collWith.botLeftPosn.y &&
				rect.botLeftPosn.y <= collWith.topRightPosn.y);
	}

	public double getStepSize() {
		return stepSize;
	}

	public void setStepSize(double stepSize) {
		this.stepSize = stepSize;
	}
	
	public ArrayList<Shape> getWorld() {
		return shapes;
	}

	public void setWorld(ArrayList<Shape> world) {
		this.shapes = world;
	}
	
	public void addShapes(ArrayList<Shape> newShapes) {
		synchronized(shapes) {
			this.shapes.addAll(newShapes);
		}
	}
	
	public void addShape(Shape newShape) {
		synchronized(shapes) {
			this.shapes.add(newShape);
		}
	}
}
